#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <vector>
#include <visualization_msgs/Marker.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include "quadrotor_msgs/PositionCommand.h"

#include "formation_avoidance/OptimizeTraj.h"


#include "bspline_cius/non_uniform_bspline_cius.h"
#include <math.h>


using namespace std;
using namespace Eigen;




trajectory_formation::NonUniformBspline BsplineTraj;
trajectory_formation::NonUniformBspline bspline_traj_vel;
trajectory_formation::NonUniformBspline bspline_traj_acc;
double BsplineTraj_start_time=0;
std::vector<std::vector<double>> headtraj;

bool stopflag=true;

bool gettrajflag=false;
std::vector<bool> getheadflag;


double current_angle=0;;
int cnt=0;

 mavros_msgs::State current_state;

bool getBspline=false;

std::vector< std::pair<double,double>> planHeadAngleKnotVector;

void mavros_state_cb(const mavros_msgs::State::ConstPtr& msg){
	 current_state = *msg;
}

void BsplineTraj_cb( formation_avoidance::OptimizeTraj msg)
{
	Eigen::MatrixXd optimize_ctrl_pts_matrix(msg.control_points.size(),3);
	for(int i=0;i<msg.control_points.size();i++)
	{
		optimize_ctrl_pts_matrix(i,0)=msg.control_points[i].x;
		optimize_ctrl_pts_matrix(i,1)=msg.control_points[i].y;
		optimize_ctrl_pts_matrix(i,2)=msg.control_points[i].z;

	}
	double ts=0.25;
	BsplineTraj.setUniformBspline(optimize_ctrl_pts_matrix,3,ts);

	Eigen::VectorXd knot(msg.knots.size());

	for(int i=0;i<msg.knots.size();i++)
	{
		knot(i)=msg.knots[i];
	}
	BsplineTraj.setKnot(knot);

	bspline_traj_vel=BsplineTraj.getDerivative();
	bspline_traj_acc=bspline_traj_vel.getDerivative();
	BsplineTraj_start_time=msg.traj_start_time.toSec();
	BsplineTraj.duration_time=BsplineTraj.getTimeSum();

	planHeadAngleKnotVector.clear();
	for(int i=0;i<msg.head_angle_knot_time.size();i++)
	{
		std::pair<double,double> knot;
		knot.first=msg.head_angle_knot_time[i].toSec();
		knot.second=msg.head_angle[i];
		planHeadAngleKnotVector.push_back(knot);
	}


	getBspline=true;
}

int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "agent_exc");
    ros::NodeHandle nh("~");

	double delay=0;
    nh.getParam("delay",delay);

	int agent_id;
    nh.getParam("agent_id",agent_id);


	int execflag;
    nh.getParam("/execflag",execflag);

	bool PlanningHeadFlag;
    nh.getParam("/PlanningHeadFlag",PlanningHeadFlag);

	int agent_num;
    nh.getParam("/AGENT_NUM",agent_num);
	getheadflag.resize(agent_num);
	headtraj.resize(agent_num);
	ROS_INFO("delay: %f >>>>>>>>>>>>>",delay);

    const int LOOPRATE = 100;
    ros::Rate loop_rate(LOOPRATE);  

    ros::Publisher  setpoint_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/planning/pos_cmd", 1);
    ros::Subscriber bspline_sub = nh.subscribe("/BsplineTraj", 1, BsplineTraj_cb);

    ros::Publisher  visualhead_pub = nh.advertise<visualization_msgs::Marker>("/visual_head", 1);

    ros::Publisher  visualhead_yaw_pub = nh.advertise<geometry_msgs::PoseStamped>("/visual_head_yaw", 1);


	int last_cnt=0;
	while(1)
	{
		if(getBspline)
		{
			double t_cur=ros::Time::now().toSec()-BsplineTraj_start_time+delay;
			t_cur=std::min(BsplineTraj.duration_time,t_cur);
			Eigen::Vector3d pos ;
			pos= BsplineTraj.evaluateDeBoorT(t_cur);
			Eigen::Vector3d vel=BsplineTraj.getDerivative().evaluateDeBoorT(t_cur);

			quadrotor_msgs::PositionCommand pos_cmd;
			pos_cmd.header.stamp=ros::Time::now();
			pos_cmd.position.x=pos(0);
			pos_cmd.position.y=pos(1);
			pos_cmd.position.z=pos(2);

			double yaw;
			if(!PlanningHeadFlag)
			{
				yaw=vel(1)/vel(0);
				yaw=std::atan2(vel(1),vel(0));
				pos_cmd.yaw=yaw;
			}
			else{

				if(ros::Time::now().toSec()<planHeadAngleKnotVector[0].first)
				{
					yaw=planHeadAngleKnotVector[0].second;
					pos_cmd.yaw=yaw;
				}
				else if(ros::Time::now().toSec()>planHeadAngleKnotVector.back().first)
				{
					yaw=planHeadAngleKnotVector.back().second;
					pos_cmd.yaw=yaw;
				}
				else{

					for(int i=0;i<planHeadAngleKnotVector.size()-1;i++)
					{
						if(ros::Time::now().toSec()>=planHeadAngleKnotVector[i].first&&ros::Time::now().toSec()<planHeadAngleKnotVector[i+1].first)
						{	
							double time_ratio=(ros::Time::now().toSec()-planHeadAngleKnotVector[i].first)/(planHeadAngleKnotVector[i+1].first-planHeadAngleKnotVector[i].first);
							yaw =(1-time_ratio)*planHeadAngleKnotVector[i].second+time_ratio*planHeadAngleKnotVector[i+1].second;
							pos_cmd.yaw=yaw;
							break;
						}
					}
				}
			}
			if(execflag>0.5)
				setpoint_pub.publish(pos_cmd);


			geometry_msgs::PoseStamped yaw_msg;
			yaw_msg.header.stamp=ros::Time::now();
			yaw_msg.pose.position.x=yaw;
			visualhead_yaw_pub.publish(yaw_msg);


			//visuliztaion head
			visualization_msgs::Marker points;
			points.id = 2346+agent_id;
			points.header.frame_id = "map";
			points.header.stamp = ros::Time::now();
			points.type = visualization_msgs::Marker::TRIANGLE_LIST;
			points.action = visualization_msgs::Marker::ADD;
			points.ns = "lines_and_points";
			points.scale.x = 1.0;
			points.scale.y = 1.0;
			points.scale.z = 1.0;
			points.color.r = 0.0f;
			points.color.g = 0.9f;
			points.color.b = 0.7f;
			points.color.a = 0.3f;
			points.lifetime = ros::Duration(0);

			Eigen::Matrix3d trans_theta1;
			trans_theta1<<cos(yaw),-sin(yaw),0,
				sin(yaw),cos(yaw),0,
				0,0,1;
			Eigen::Vector3d triangle1[3];
			triangle1[0]=pos;
			double hfov{1.047};
			double MaxCameraDis{5.0};
			double hmax=MaxCameraDis*tan(hfov/2)*2;
			double vmax=hmax/640.0*480.0;
			for(int i=1;i<=2;i++)
			{
				Eigen::Vector3d corner(MaxCameraDis,hmax/2.0*pow(-1,i+1),0);
				Eigen::Vector3d vector1=pos+trans_theta1*corner;
				triangle1[i]<<vector1(0),vector1(1),vector1(2);
			}

			geometry_msgs::Point point;
			point.x = triangle1[0](0);
			point.y = triangle1[0](1);
			point.z = triangle1[0](2);
			// ROS_INFO("point: %f %f %f",point.x,point.y,point.z);
			points.points.push_back(point);  


			point.x = triangle1[1](0);
			point.y = triangle1[1](1);
			point.z = triangle1[1](2);
			points.points.push_back(point);  


			point.x = triangle1[2](0);
			point.y = triangle1[2](1);
			point.z = triangle1[2](2);
			points.points.push_back(point);  
			visualhead_pub.publish(points);

		}

		// double time=ros::Time::now().toSec();
		// ROS_INFO("id: %d current  time: %f",agent_id ,time);
		ros::spinOnce();
		loop_rate.sleep();

	}

    // Eigen::Vector2f triangle1[3]
}
